#include <rtthread.h>
#include <rtdevice.h>

#include "ov7725_regs.h"

struct regval
{
	uint8_t reg;
	uint8_t value;
};

#define ENDMARKER  \
	{              \
		0x00, 0x00 \
	}

#define CIF_WIDTH (400)
#define CIF_HEIGHT (296)

#define SVGA_WIDTH (800)
#define SVGA_HEIGHT (600)

#define QVGA_WIDTH (320)
#define QVGA_HEIGHT (240)

#define UXGA_WIDTH (1600)
#define UXGA_HEIGHT (1200)

#define OMV_OV7725_BANDING 0x7f

static const struct regval ov2640_init_regs[] = {
	{COM12, 0x03},
	{HSTART, 0x22},
	{HSIZE, 0xa4},
	{VSTART, 0x07},
	{VSIZE, 0xf0},
	{HREF, 0x00},
	{HOUTSIZE, 0xa0},
	{VOUTSIZE, 0xf0},
	{EXHCH, 0x00},
	{CLKRC, 0xC0}, // {CLKRC, 0x01},

	{TGT_B, 0x7f},
	{FIXGAIN, 0x09},
	{AWB_CTRL0, 0xe0},
	{DSP_CTRL1, 0xff},
	{DSP_CTRL2, 0x20 | DSP_CTRL2_VDCW_EN | DSP_CTRL2_HDCW_EN | DSP_CTRL2_VZOOM_EN | DSP_CTRL2_HZOOM_EN}, // {DSP_CTRL2, 0x20},
	{DSP_CTRL3, 0x00},
	{DSP_CTRL4, 0x48},

	{COM8, 0xf0},
	{COM4, 0x41}, // {COM4, 0x41},
	{COM6, 0xc5},
	{COM9, 0x11},
	{BDBASE, 0x7f},
	{BDSTEP, 0x03},
	{AEW, 0x40},
	{AEB, 0x30},
	{VPT, 0xa1},
	{EXHCL, 0x00},
	{AWB_CTRL3, 0xaa},
	{COM8, 0xff},

	{EDGE1, 0x05},
	{DNSOFF, 0x01},
	{EDGE2, 0x03},
	{EDGE3, 0x00},
	{MTX1, 0xb0},
	{MTX2, 0x9d},
	{MTX3, 0x13},
	{MTX4, 0x16},
	{MTX5, 0x7b},
	{MTX6, 0x91},
	{MTX_CTRL, 0x1e},
	{BRIGHTNESS, 0x08},
	{CONTRAST, 0x20},
	{UVADJ0, 0x81},
	{SDE, SDE_CONT_BRIGHT_EN | SDE_SATURATION_EN},

	{GAM1, 0x0c},
	{GAM2, 0x16},
	{GAM3, 0x2a},
	{GAM4, 0x4e},
	{GAM5, 0x61},
	{GAM6, 0x6f},
	{GAM7, 0x7b},
	{GAM8, 0x86},
	{GAM9, 0x8e},
	{GAM10, 0x97},
	{GAM11, 0xa4},
	{GAM12, 0xaf},
	{GAM13, 0xc5},
	{GAM14, 0xd7},
	{GAM15, 0xe8},
	{SLOP, 0x20},

	{DM_LNL, 0x00},
	{BDBASE, OMV_OV7725_BANDING}, // {BDBASE, 0x7f}
	{BDSTEP, 0x03},

	{LC_RADI, 0x10},
	{LC_COEF, 0x10},
	{LC_COEFB, 0x14},
	{LC_COEFR, 0x17},
	{LC_CTR, 0x01}, // {LC_CTR, 0x05},

	{COM5, 0xf5}, // {COM5, 0x65},

	ENDMARKER,
};

static int reg_read(rt_video_t *vid, uint16_t reg, uint8_t *d)
{
	int ret = 0;

	*d = 0;
	ret |= rt_camif_reg_read(vid->ci, vid->ci->slv_addr, reg, d);

	return ret;
}

static int reg_write(rt_video_t *vid, uint16_t reg, uint8_t d)
{
	return rt_camif_reg_write(vid->ci, vid->ci->slv_addr, reg, &d);
}

static int reg_read_e1(rt_video_t *vid, uint16_t reg, uint8_t *d)
{
	reg_read(vid, reg, d);

	return ((*d != 0) && (*d != 0xff));
}

static int ov2640_write_array(rt_video_t *vid, const struct regval *vals)
{
	int ret = 0;

	while ((vals->reg != 0x00) || (vals->value != 0x00))
	{
		ret = reg_write(vid, vals->reg, vals->value);
		if (ret != 0)
		{
			break;
		}

		vals++;
	}

	return ret;
}

static int ov2640_mask_set(rt_video_t *vid, uint16_t reg, uint8_t mask, uint8_t set)
{
	uint8_t val;

	if (reg_read(vid, reg, &val) != 0)
		return -1;

	val &= ~mask;
	val |= set & mask;

	return reg_write(vid, reg, val);
}

static int _set_img_format(rt_video_t *vid, struct video_format *fmt)
{
	int ret = 0;
	struct video_pix_format *pf = &fmt->fmt.pix;
    uint8_t reg;

    ret = reg_read(vid, COM7, &reg);

	switch (pf->pixelformat)
	{
	case VIDEO_PIX_FMT_RGB565:
	{
        reg = COM7_SET_FMT(reg, COM7_FMT_RGB);
        ret |= reg_write(vid, DSP_CTRL4, DSP_CTRL4_YUV_RGB);
	}
	break;
	default:
	{
		ret = -EINVAL;
	}
	break;
	}

    ret |= reg_write(vid, COM7, reg);

	return ret;
}

static int _set_img_size(rt_video_t *vid, struct video_format *fmt)
{
	int ret = 0;
	uint16_t w, h;
	uint8_t reg;
	int vflip;

	w = fmt->fmt.pix.width;
	h = fmt->fmt.pix.height;

	if ((w > 640) || (h > 480))
	{
		return -1;
	}

	// Write MSBs
	ret |= reg_write(vid, HOUTSIZE, w >> 2);
	ret |= reg_write(vid, VOUTSIZE, h >> 1);

	// Write LSBs
	ret |= reg_write(vid, EXHCH, ((w & 0x3) | ((h & 0x1) << 2)));

	// Sample VFLIP
	ret |= reg_write(vid, COM3, reg);
	vflip = reg & COM3_VFLIP;
	ret |= reg_write(vid, HREF, reg);
	ret |= reg_write(vid, HREF, (reg & 0xBF) | (vflip ? 0x40 : 0x00));

	if ((w <= 320) && (h <= 240))
	{
		// Set QVGA Resolution
		ret = reg_read(vid, COM7, &reg);
		reg = COM7_SET_RES(reg, COM7_RES_QVGA);
		ret |= reg_write(vid, COM7, reg);

		// Set QVGA Window Size
		ret |= reg_write(vid, HSTART, 0x3F);
		ret |= reg_write(vid, HSIZE, 0x50);
		ret |= reg_write(vid, VSTART, 0x03 - vflip);
		ret |= reg_write(vid, VSIZE, 0x78);

		// Enable auto-scaling/zooming factors
		ret |= reg_write(vid, DSPAUTO, 0xFF);
	}

	return ret;
}

static int reg_read16(rt_camif_t *ci, uint16_t reg, uint16_t *d)
{
	uint8_t tmp = 0;
	int ret = 0;

	*d = 0;
	ret |= rt_camif_reg_read(ci, ci->slv_addr, reg, &tmp);
	*d = tmp << 8;
	ret |= rt_camif_reg_read(ci, ci->slv_addr, reg + 1, &tmp);
	*d |= tmp;

	return ret;
}

static int _readid(rt_camif_t *ci, uint32_t *id)
{
	uint16_t val;
	int ret = -1;

	reg_read16(ci, 0x0A, &val);

	if ((val == 0x7720) || (val == 0x7721))
	{
		ret = 0;
		if (id)
			*id = val;
	}

	return ret;
}

static int ov_init(rt_video_t *vid)
{
	int ret = 0;

	rt_camif_set_xclk(vid->ci, 12000000);

	ret |= reg_write(vid, COM7, COM7_RESET);

	rt_thread_mdelay(20);

	ret = ov2640_write_array(vid, ov2640_init_regs);

	rt_thread_mdelay(200);

	return ret;
}

static void ov_deinit(rt_video_t *vid)
{
}

static int ov_set_format(rt_video_t *vid, struct video_format *fmt)
{
	int ret;

	ret = _set_img_size(vid, fmt);
	if (ret != 0)
		goto _out;

	ret = _set_img_format(vid, fmt);

_out:
	return ret;
}

static int ov_get_format(rt_video_t *vid, struct video_format *fmt)
{
	int ret = -EINVAL;
	uint16_t w, h;
#if 0
    if (!reg_read_e1(vid, 0x3808, &w)) //width
        goto _out;
    if (!reg_read_e1(vid, 0x380A, &h)) //height
        goto _out;
#endif
	fmt->fmt.pix.width = 320;
	fmt->fmt.pix.height = 240;
	fmt->fmt.pix.bytesperline = fmt->fmt.pix.width * 2;
	ret = 0;

_out:
	return ret;
}

static int ov_stream_enable(rt_video_t *vid, int en)
{
	return 0;
}

static const struct video_ops _ops =
	{
		.init = ov_init,
		.deinit = ov_deinit,
		.set_format = ov_set_format,
		.get_format = ov_get_format,
		.stream_enable = ov_stream_enable,
};

int ov7725_probe(rt_camif_t *ci, const struct video_ops **ops, uint32_t *id)
{
	int ret;

	ci->slv_addr = 0x42;

	ret = _readid(ci, id);
	if (ret == 0 && ops)
	{
		*ops = &_ops;
	}

	return ret;
}
